#pragma warning (disable : 4251 4996 4819 4244)
#include "MapProxyStateValidityChecker.h"
#include "StateRecorder.h"
#include "PathPlanner.h"
#include "ptRRT.h"

using namespace ompl::base;
using namespace ompl::geometric;

PathPlanner::PathPlanner() : ss( NULL )
{
}

PathPlanner::~PathPlanner()
{
}

PathPlanner& PathPlanner::Instance()
{
    static PathPlanner* pInstance = new PathPlanner();

    return *pInstance;
}

void PathPlanner::Init( const PlayerCc::MapProxy &map, const boost::shared_ptr<FrictionModel> frictionModel, const bool bUsePRRT)
{
    m_bUsePRRT = bUsePRRT;

    double xmax = map.GetWidth() * map.GetResolution() / 2.0;
    double ymax = map.GetHeight() * map.GetResolution() / 2.0;

    StateSpacePtr space(new SE2StateSpace());
    RealVectorBounds bounds(2);
    bounds.setLow(0, -xmax); bounds.setHigh(0, xmax);
    bounds.setLow(1, -ymax); bounds.setHigh(1, ymax);
    space->as<SE2StateSpace>()->setBounds(bounds);
    ss = new SimpleSetup(space);

    // set validity checker
    StateValidityCheckerPtr checker(new MapProxyStateValidityChecker(0.4, ss->getSpaceInformation(), map));
    ss->setStateValidityChecker(checker);

    if ( m_bUsePRRT )
    {
        PlannerPtr planner(new ptRRT(ss->getSpaceInformation(), frictionModel));
        ss->setPlanner(planner);
    }
    else
    {
        PlannerPtr planner(new RRT(ss->getSpaceInformation()));
        ss->setPlanner(planner);
    }
}

ompl::geometric::PathGeometric& PathPlanner::Plan( bool& outSolved, const double dStartX, const double dStartY, const double dEndX, const double dEndY, const double dStepRange )
{
    if ( m_bUsePRRT )
    {
        ss->getPlanner()->as<ptRRT>()->setRange( dStepRange );
    }
    else
    {
        ss->getPlanner()->as<RRT>()->setRange( dStepRange );
    }

    // start at the existing position
    ScopedState<SE2StateSpace> start(ss->getSpaceInformation());
    start->setX(dStartX);
    start->setY(dStartY);

    // goal is at order table position
    ScopedState<SE2StateSpace> goal(ss->getSpaceInformation());
    goal->setX(dEndX); 
    goal->setY(dEndY);

    // set start and goal
    ss->setStartAndGoalStates(start, goal);

    // setup path planner
    ss->clear();
    ss->setup();
    //ss.print(std::cout);

    // plan
    PlannerStatus solved = ss->solve(5.0);

    record_state( ss->getSolutionPath(), "Solution.csv" );

    PlannerData oPlannerData( ss->getSpaceInformation() );
    ss->getPlanner()->getPlannerData( oPlannerData );

    std::vector<State*> vecStates;
    for ( unsigned int nLoop = 0; nLoop < oPlannerData.numVertices(); ++nLoop )
    {
        vecStates.push_back((State*)(oPlannerData.getVertex( nLoop ).getState()));
    }
    record_state( vecStates, "Solution.plantree.csv" );

    outSolved = solved;

    return ss->getSolutionPath();
}

bool PathPlanner::Plan( std::list<Position>& outSolution, const double dStartX, const double dStartY, const double dEndX, const double dEndY, const double dStepRange )
{
    bool bSolved = false;

    ompl::geometric::PathGeometric& refSolution = PathPlanner::Instance().Plan( bSolved, dStartX, dStartY, dEndX, dEndY, dStepRange );

    outSolution.clear();
    if ( bSolved )
    {
        std::size_t stateCount = refSolution.getStateCount();

        for (std::size_t i = 0; i < stateCount; i++)
        {
            const ompl::base::SE2StateSpace::StateType * state = refSolution.getState(i)->as<ompl::base::SE2StateSpace::StateType>();
            Position stPos = { state->getX(), state->getY(), state->getYaw() };
            outSolution.push_back( stPos );
        }
    }

    return bSolved;
}

bool PathPlanner::IsPosValid( const double dX, const double dY )
{
    ScopedState<SE2StateSpace> cg(ss->getSpaceInformation());
    cg->setX(dX); cg->setY(dY);

    return ss->getStateValidityChecker()->isValid(cg.get());
}

void PathPlanner::setVisualizer(boost::shared_ptr<Visualizer> visualizer) {
    if ( m_bUsePRRT )
    {
        ss->getPlanner()->as<ptRRT>()->setVisualizer( visualizer );
    }
}
